/* ROBOT DEMO */



#include <sys/types.h>

#include <sys/stat.h>

#include <fcntl.h>

#include <termios.h>

#include <stdio.h>

#include <unistd.h>

#include <strings.h>

#include <stdlib.h>



#define BAUDRATE B57600  /* the baud rate */
//#define MODEMDEVICE "/dev/ttyUSB0" /* port for serial port */
#define MODEMDEVICE "/dev/rfcomm0" /* port for bluetooth connection */
#define _POSIX_SOURCE 1 /* POSIX compliant source */





int main()

{

	printf("ROBOT DEMO\n");



	int c, res, fd;
	char buf[6], buf2[6]; /* buffers used for read and write to irobot */

	char key;


	struct termios oldtio,newtio;





	/* Configure Serial port */

	/* Open modem device for reading and writing */

	fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY ); 

	if (fd <0) {

		printf("Error Connecting Device‏‏\n");

		perror(MODEMDEVICE); 

		exit(-1); 

	}



	tcgetattr(fd,&oldtio); /* save current serial port settings */

	bzero(&newtio, sizeof(newtio)); /* clear struct for new port settings */

	

	newtio.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;

	newtio.c_iflag = IGNPAR | ICRNL;

	newtio.c_oflag = 0;

	newtio.c_lflag = ICANON;

	

	/* initialize all control characters */

	newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */ 

	newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */

	newtio.c_cc[VERASE]   = 0;     /* del */

	newtio.c_cc[VKILL]    = 0;     /* @ */

	newtio.c_cc[VEOF]     = 4;     /* Ctrl-d */

	newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */

	newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives */

	newtio.c_cc[VSWTC]    = 0;     /* '\0' */

	newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */ 

	newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */

	newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */

	newtio.c_cc[VEOL]     = 0;     /* '\0' */

	newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */

	newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */

	newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */

	newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */

	newtio.c_cc[VEOL2]    = 0;     /* '\0' */



	/* now clean the modem line and activate the settings for the port */

	tcflush(fd, TCIFLUSH);

	tcsetattr(fd,TCSANOW,&newtio);


	/* initial start sequence*/

	buf[0] = 128;

	buf[1] = 131;

	write(fd, buf, 2);


	printf("Press 'f' to go forward, 'b' to go backward‏‏, or 's' to stop\n");

	while (1) {

		key = getc(stdin); //gets character from stdin

		if (key == 'f') {

			printf("%c was keyed\n",key);

			//write bytes into global buffer that will tell robot to move

			buf2[0] = 137;

			buf2[1] = 0;

			buf2[2] = 100;

			buf2[3] = 128;

			buf2[4] = 0;

		}

		if (key == 'b') {

			printf("%c was keyed\n",key);

			buf2[0] = 137;

			buf2[1] = 255;

			buf2[2] = 100;

			buf2[3] = 128;

			buf2[4] = 0;

		}

		if (key == 's') {

			printf("%c was keyed\n",key);

			buf2[0] = 137;

			buf2[1] = 0;

			buf2[2] = 0;

			buf2[3] = 0;

			buf2[4] = 0;

		}

		if (key == 'c') exit(0);//terminates the program



		printf("Sending Message to Robot...\n");

		write(fd, buf, 2);//sequence packets for movement

		write(fd, buf2, 5);

	}

	exit(0);

}


